#include "aiming_cmd.h"
#include "oslib_uart.h"
#include "boxReader.h"
#include "dji2_0.h"
#include "utils.h"
#include "cmsis_os2.h"
#include "math.h"
#include <stdlib.h>

void setAimingId(int argc, char *argv[])
{
    if (argc != 2)
    {
        uprintf("params is error!\r\n");
        return;
    }
    int nums = atoi(argv[1]);
    if (nums < 0 || nums > 29)
    {
        uprintf("aimingId should be in range of (1,29)\r\n");
        return;
    }
    aimingId = nums;
    PID_Reset(&aimingPid);
    uprintf("aimingId is set to %d\r\n", aimingId);
}

void setAutoAiming(int argc, char *argv[])
{
    changeAuto();
    uprintf("change to auto aiming mode\r\n");
}

void setManualAiming(int argc, char *argv[])
{
    changeManul();
    uprintf("change to manul aiming mode\r\n");
}

void commandTestLeft(int argc, char *argv[])
{
    manualLeft = !manualLeft;
    uprintf("Left changes to %d \r\n", manualLeft);
}

void commandTestRight(int argc, char *argv[])
{
    manualRight = !manualRight;
    uprintf("Right changes to %d \r\n", manualRight);
}

void commandHoming(int argc, char *argv[])
{
    if (argc != 4)
    {
        uprintf("Param num is error");
        return;
    }
    int num = atoi(argv[1]);
    int vel = atoi(argv[2]);
    int cur = atoi(argv[3]);
    if (num < 0 || num > 4)
    {
        uprintf("illegal motorId [%d]\r\n", num);
        return;
    }
    if (fabsf(cur) > 3000)
    {
        uprintf("cur is too huge!(< 3)\r\n");
        return;
    }
    if (abs(vel) > 500)
    {
        uprintf("vel is too huge!(< 500)\r\n");
        return;
    }
    dji_Homing(TRIPOD_HCAN, TRIPOD_BOARD_ID, num, vel, cur);
    uprintf("motor%d's mode changes to homingmode\r\n", num);
}

void commandRunning(int argc, char *argv[])
{
    if (running == 1) {
        uprintf("RUNNING\n");
    }
    else {
        uprintf("NOT RUNNING\n");
    }
}

void commandManualArrive(int argc, char *argv[])
{
    if (0 == manualArrive()) {
        uprintf("Command manual Arrive\n");
    }
    else {
        uprintf("Failed to manual arrive\n");
    }
}

void commandPID(int argc, char *argv[])
{
    if (argc == 5) {
        aimingPid.Kp = atof(argv[1]);
        aimingPid.Ki = atof(argv[2]);
        aimingPid.Kd = atof(argv[3]);
        aimingPid.int_max = atof(argv[4]);
        aimingPid.int_sum = 0;
        aimingPid.last_err = 0;
        aimingPid.last_last_err = 0;
        aimingPid.last_delta_err = 0;
        uprintf("KP=%.2f, KI=%.2f, KD=%.2f, i_max=%.2f\n", aimingPid.Kp, aimingPid.Ki, aimingPid.Kd, aimingPid.int_max);
    }
    else {
        uprintf("Error\n");
    }
}